close all
data = csvread('p_v_a_psp_vsp_prysp.csv',30,0);
data = data(1:end,:);
t = data(:,1);
p = data(:,2:4);
v = data(:,5:7);
a = data(:,8:10);
psp = data(:,12:14);
vsp = data(:,15:17);
anglesp = data(:,18:20);
angle = data(:,22:24);
v_cam = data(:,25:27);


figure
subplot(3,3,1);
plot(t,p);
legend('p_x','p_y','p_z');
grid on
subplot(3,3,2);
plot(t,v);
legend('v_x','v_y','v_z');
grid on
subplot(3,3,3);
plot(t,a);
legend('a_x','a_y','a_z');
grid on
subplot(3,3,4);
plot(t,psp);
legend('psp_x','psp_y','psp_z');
grid on
subplot(3,3,5);
plot(t,vsp);
legend('vsp_x','vsp_y','vsp_z');
grid on
subplot(3,3,6);
plot(t,anglesp);
legend('anglesp_x','anglesp_y','anglesp_z');
grid on
subplot(3,3,9);
plot(t,angle);
legend('angle_x','angle_y','angle_z');
grid on

% figure 
% subplot(2,1,1);
% plot(t,[p(:,2),v(:,2),vsp(:,2)]);
% legend('p_y','v_y','vsp_y');
% grid on
% subplot(2,1,2);
% plot(t,[vsp(:,2),anglesp(:,2),angle(:,2)]);
% legend('vsp_y','anglesp_y','angle_y');
% grid on

figure 
subplot(2,1,1);
plot(t,[p(:,2),v(:,2),vsp(:,2)]);
legend('p_y','v_y','vsp_y');
grid on
subplot(2,1,2);
plot(t,[v(:,2),v_cam(:,2)]);
legend('v_y','vCam_y','a_y');
grid on

figure 
subplot(2,1,1);
plot(t,[p(:,3),v(:,3),vsp(:,3)]);
legend('p_z','v_z','vsp_z');
grid on
subplot(2,1,2);
plot(t,[v_cam(:,3),v(:,3)]);
legend('vCam_z','v_z','a_z');
grid on
figure 
subplot(2,1,1);
plot(t,[p(:,1),v(:,1),vsp(:,1)]);
legend('p_x','v_x','vsp_x');
grid on
subplot(2,1,2);
plot(t,[v_cam(:,1),v(:,1)]);
legend('vCam_x','v_x','a_x');
grid on

% 
% figure
% % plot(t,[p(:,2),v(:,2),vsp(:,2)]);
% % legend('p_y','v_y','vsp_y');
% plot(t,[p(:,2),v(:,2),vsp(:,2),9.8*(anglesp(:,2)/180*pi)]);
% legend('p','v','vsp','angleToAccsp');
% grid on
% figure
% plot(t,[p(:,3),v(:,3),vsp(:,3)]);
% legend('p_z','v_z','vsp_z');
% grid on
% 
% figure
% plot(t,[p(:,1),v(:,1),vsp(:,1)]);
% legend('p_x','v_x','vsp_x');
% grid on

% plot(t,[9.8*(anglesp(:,2)/180*pi),a(:,2),v(:,2)]);
% legend('RollToAccsp','acc_y','v_y');
% grid on